/******************************************************************************
 * Copyright 2022 The AIR Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include <string>
#include "v2x-asn-msgs-internal.h"

bool motion_asn2pb(const MotionConfidenceSet_t* asn_motion,
                   ::v2xpb::asn::MotionConfidenceSet* pb_motion) {
  if (!asn_motion || !pb_motion) {
    LOG(WARNING) << "args false";
    return false;
  }

  if (asn_motion->speedCfd) {
    pb_motion->set_speed_cfd(static_cast<int>(*asn_motion->speedCfd));
  }
  if (asn_motion->headingCfd) {
    pb_motion->set_heading_cfd(static_cast<int>(*asn_motion->headingCfd));
  }
  if (asn_motion->steerCfd) {
    pb_motion->set_steer_cfd(static_cast<int>(*asn_motion->steerCfd));
  }

  return true;
}

bool motion_pb2asn(const ::v2xpb::asn::MotionConfidenceSet& pb_motion,
                   MotionConfidenceSet_t* asn_motion) {
  if (!asn_motion) {
    LOG(WARNING) << "args false";
    return false;
  }

  if (pb_motion.has_speed_cfd()) {
    asn_motion->speedCfd = ASN1(SpeedConfidence)::create_tp();
    *asn_motion->speedCfd = pb_motion.speed_cfd();
  }
  if (pb_motion.has_heading_cfd()) {
    asn_motion->headingCfd = ASN1(HeadingConfidence)::create_tp();
    *asn_motion->headingCfd = pb_motion.heading_cfd();
  }
  if (pb_motion.has_steer_cfd()) {
    asn_motion->steerCfd = ASN1(SteeringWheelAngleConfidence)::create_tp();
    *asn_motion->steerCfd = pb_motion.steer_cfd();
  }
  return true;
}

bool safety_events_asn2pb(const VehicleEventFlags_t* events_asn,
                          google::protobuf::RepeatedField<int>* events_pb) {
  if (!events_asn || !events_pb) {
    LOG(INFO) << "args false";
    return false;
  }

  ASN1_HOLDER(VehicleEventFlags) const events(events_asn);

  if (events.test_bit(VehicleEventFlags_eventHazardLights)) {
    events_pb->Add(::v2xpb::asn::EVENT_HAZARD_LIGHTS);
  }
  if (events.test_bit(VehicleEventFlags_eventStopLineViolation)) {
    events_pb->Add(::v2xpb::asn::EVENT_STOP_LINEVIOLATIO);
  }
  if (events.test_bit(VehicleEventFlags_eventABSactivated)) {
    events_pb->Add(::v2xpb::asn::EVENT_ABS_ACTIVATED);
  }
  if (events.test_bit(VehicleEventFlags_eventTractionControlLoss)) {
    events_pb->Add(::v2xpb::asn::EVENT_TRACTION_CONTROL_LOSS);
  }
  if (events.test_bit(VehicleEventFlags_eventStabilityControlactivated)) {
    events_pb->Add(::v2xpb::asn::EVENT_STABILITY_CONTROL_ACTIVATED);
  }
  if (events.test_bit(VehicleEventFlags_eventHazardousMaterials)) {
    events_pb->Add(::v2xpb::asn::EVENT_HAZARDOUS_MATERIALS);
  }
  if (events.test_bit(VehicleEventFlags_eventReserved1)) {
    events_pb->Add(::v2xpb::asn::EVENT_RESERVEDL);
  }
  if (events.test_bit(VehicleEventFlags_eventHardBraking)) {
    events_pb->Add(::v2xpb::asn::EVENT_HARDBRAKING);
  }
  if (events.test_bit(VehicleEventFlags_eventLightsChanged)) {
    events_pb->Add(::v2xpb::asn::EVENT_LIGHTS_CHANGED);
  }
  if (events.test_bit(VehicleEventFlags_eventWipersChanged)) {
    events_pb->Add(::v2xpb::asn::EVENT_WIPERS_CHAGED);
  }
  if (events.test_bit(VehicleEventFlags_eventFlatTire)) {
    events_pb->Add(::v2xpb::asn::EVENT_FLAT_TIRE);
  }
  if (events.test_bit(VehicleEventFlags_eventDisabledVehicle)) {
    events_pb->Add(::v2xpb::asn::EVENT_DISABLED_VEHICLE);
  }
  if (events.test_bit(VehicleEventFlags_eventAirBagDeployment)) {
    events_pb->Add(::v2xpb::asn::EVENT_AIR_BAG_DEPLOYMENT);
  }

  return true;
}

bool safety_events_pb2asn(const google::protobuf::RepeatedField<int>& events_pb,
                          VehicleEventFlags_t* events_asn) {
  if (!events_asn) {
    LOG(INFO) << "args false";
    return false;
  }

  ASN1_HOLDER(VehicleEventFlags) events(events_asn);
  events.init();

  for (const auto& event_pb : events_pb) {
    switch (event_pb) {
      case ::v2xpb::asn::EVENT_HAZARD_LIGHTS:
        events.set_bit(VehicleEventFlags_eventHazardLights);
        break;
      case ::v2xpb::asn::EVENT_STOP_LINEVIOLATIO:
        events.set_bit(VehicleEventFlags_eventStopLineViolation);
        break;
      case ::v2xpb::asn::EVENT_ABS_ACTIVATED:
        events.set_bit(VehicleEventFlags_eventABSactivated);
        break;
      case ::v2xpb::asn::EVENT_TRACTION_CONTROL_LOSS:
        events.set_bit(VehicleEventFlags_eventTractionControlLoss);
        break;
      case ::v2xpb::asn::EVENT_STABILITY_CONTROL_ACTIVATED:
        events.set_bit(VehicleEventFlags_eventStabilityControlactivated);
        break;
      case ::v2xpb::asn::EVENT_HAZARDOUS_MATERIALS:
        events.set_bit(VehicleEventFlags_eventHazardousMaterials);
        break;
      case ::v2xpb::asn::EVENT_RESERVEDL:
        events.set_bit(VehicleEventFlags_eventReserved1);
        break;
      case ::v2xpb::asn::EVENT_HARDBRAKING:
        events.set_bit(VehicleEventFlags_eventHardBraking);
        break;
      case ::v2xpb::asn::EVENT_LIGHTS_CHANGED:
        events.set_bit(VehicleEventFlags_eventLightsChanged);
        break;
      case ::v2xpb::asn::EVENT_WIPERS_CHAGED:
        events.set_bit(VehicleEventFlags_eventWipersChanged);
        break;
      case ::v2xpb::asn::EVENT_FLAT_TIRE:
        events.set_bit(VehicleEventFlags_eventFlatTire);
        break;
      case ::v2xpb::asn::EVENT_DISABLED_VEHICLE:
        events.set_bit(VehicleEventFlags_eventDisabledVehicle);
        break;
      case ::v2xpb::asn::EVENT_AIR_BAG_DEPLOYMENT:
        events.set_bit(VehicleEventFlags_eventAirBagDeployment);
        break;
    }
  }
  return true;
}

bool safety_lights_asn2pb(const ExteriorLights_t* lights_asn,
                          google::protobuf::RepeatedField<int>* lights_pb) {
  if (!lights_asn || !lights_pb) {
    LOG(INFO) << "args false";
    return false;
  }

  ASN1_HOLDER(ExteriorLights) const lights(lights_asn);

  if (lights.test_bit(ExteriorLights_lowBeamHeadlightsOn)) {
    lights_pb->Add(::v2xpb::asn::LIGHT_LOW_BEAM_HEAD);
  }
  if (lights.test_bit(ExteriorLights_highBeamHeadlightsOn)) {
    lights_pb->Add(::v2xpb::asn::LIGHT_HIGHT_BEAM_HEAD);
  }
  if (lights.test_bit(ExteriorLights_leftTurnSignalOn)) {
    lights_pb->Add(::v2xpb::asn::LIGHT_LEFT_TURN);
  }
  if (lights.test_bit(ExteriorLights_rightTurnSignalOn)) {
    lights_pb->Add(::v2xpb::asn::LIGHT_RIGHT_TURN);
  }
  if (lights.test_bit(ExteriorLights_hazardSignalOn)) {
    lights_pb->Add(::v2xpb::asn::LIGHT_HAZARD);
  }
  if (lights.test_bit(ExteriorLights_automaticLightControlOn)) {
    lights_pb->Add(::v2xpb::asn::LIGHT_AUTOM_ATIC_LIGHT_CONTROL);
  }
  if (lights.test_bit(ExteriorLights_daytimeRunningLightsOn)) {
    lights_pb->Add(::v2xpb::asn::LIGHT_DAYTIME_RUNNING);
  }
  if (lights.test_bit(ExteriorLights_fogLightOn)) {
    lights_pb->Add(::v2xpb::asn::LIGHT_FOG);
  }
  if (lights.test_bit(ExteriorLights_parkingLightsOn)) {
    lights_pb->Add(::v2xpb::asn::LIGHT_PARKING);
  }

  return true;
}

bool safety_lights_pb2asn(const google::protobuf::RepeatedField<int>& lights_pb,
                          VehicleEventFlags_t* lights_asn) {
  if (!lights_asn) {
    LOG(INFO) << "args false";
    return false;
  }

  ASN1_HOLDER(VehicleEventFlags) lights(lights_asn);
  lights.init();

  for (const auto& light_pb : lights_pb) {
    switch (light_pb) {
      case ::v2xpb::asn::LIGHT_LOW_BEAM_HEAD:
        lights.set_bit(ExteriorLights_lowBeamHeadlightsOn);
        break;
      case ::v2xpb::asn::LIGHT_HIGHT_BEAM_HEAD:
        lights.set_bit(ExteriorLights_highBeamHeadlightsOn);
        break;
      case ::v2xpb::asn::LIGHT_LEFT_TURN:
        lights.set_bit(ExteriorLights_leftTurnSignalOn);
        break;
      case ::v2xpb::asn::LIGHT_RIGHT_TURN:
        lights.set_bit(ExteriorLights_rightTurnSignalOn);
        break;
      case ::v2xpb::asn::LIGHT_HAZARD:
        lights.set_bit(ExteriorLights_hazardSignalOn);
        break;
      case ::v2xpb::asn::LIGHT_AUTOM_ATIC_LIGHT_CONTROL:
        lights.set_bit(ExteriorLights_automaticLightControlOn);
        break;
      case ::v2xpb::asn::LIGHT_DAYTIME_RUNNING:
        lights.set_bit(ExteriorLights_daytimeRunningLightsOn);
        break;
      case ::v2xpb::asn::LIGHT_FOG:
        lights.set_bit(ExteriorLights_fogLightOn);
        break;
      case ::v2xpb::asn::LIGHT_PARKING:
        lights.set_bit(ExteriorLights_parkingLightsOn);
        break;
    }
  }
  return true;
}

bool gnss_status_asn2pb(const GNSSstatus_t* gnss_asn,
                        google::protobuf::RepeatedField<int>* gnss_pb) {
  if (!gnss_asn || !gnss_pb) {
    LOG(INFO) << "args false";
    return false;
  }

  ASN1_HOLDER(GNSSstatus) const gnss(gnss_asn);

  if (gnss.test_bit(GNSSstatus_unavailable)) {
    gnss_pb->Add(::v2xpb::asn::GNSS_UNAVAILABLE);
  }
  if (gnss.test_bit(GNSSstatus_isHealthy)) {
    gnss_pb->Add(::v2xpb::asn::GNSS_HEALTHY);
  }
  if (gnss.test_bit(GNSSstatus_isMonitored)) {
    gnss_pb->Add(::v2xpb::asn::GNSS_MONITORED);
  }
  if (gnss.test_bit(GNSSstatus_baseStationType)) {
    gnss_pb->Add(::v2xpb::asn::GNSS_BASE_STATION_TYPE);
  }
  if (gnss.test_bit(GNSSstatus_aPDOPofUnder5)) {
    gnss_pb->Add(::v2xpb::asn::GNSS_APDOP_OF_UNDER);
  }
  if (gnss.test_bit(GNSSstatus_inViewOfUnder5)) {
    gnss_pb->Add(::v2xpb::asn::GNSS_INVIEW_OF_UNDER);
  }
  if (gnss.test_bit(GNSSstatus_localCorrectionsPresent)) {
    gnss_pb->Add(::v2xpb::asn::GNSS_LOCAL_CORRECTIONS_PRESENT);
  }
  if (gnss.test_bit(GNSSstatus_networkCorrectionsPresent)) {
    gnss_pb->Add(::v2xpb::asn::GNSS_NETWORK_CORRECTIONS_PRESENT);
  }

  return true;
}

bool gnss_status_pb2asn(const google::protobuf::RepeatedField<int>& gnss_pb,
                        GNSSstatus_t* gnss_asn) {
  if (!gnss_asn) {
    LOG(INFO) << "args false";
    return false;
  }

  ASN1_HOLDER(GNSSstatus) gnss(gnss_asn);
  gnss.init();

  for (const auto& gn_pb : gnss_pb) {
    switch (gn_pb) {
      case ::v2xpb::asn::GNSS_UNAVAILABLE:
        gnss.set_bit(GNSSstatus_unavailable);
        break;
      case ::v2xpb::asn::GNSS_HEALTHY:
        gnss.set_bit(GNSSstatus_isHealthy);
        break;
      case ::v2xpb::asn::GNSS_MONITORED:
        gnss.set_bit(GNSSstatus_isMonitored);
        break;
      case ::v2xpb::asn::GNSS_BASE_STATION_TYPE:
        gnss.set_bit(GNSSstatus_baseStationType);
        break;
      case ::v2xpb::asn::GNSS_APDOP_OF_UNDER:
        gnss.set_bit(GNSSstatus_aPDOPofUnder5);
        break;
      case ::v2xpb::asn::GNSS_INVIEW_OF_UNDER:
        gnss.set_bit(GNSSstatus_inViewOfUnder5);
        break;
      case ::v2xpb::asn::GNSS_LOCAL_CORRECTIONS_PRESENT:
        gnss.set_bit(GNSSstatus_localCorrectionsPresent);
        break;
      case ::v2xpb::asn::GNSS_NETWORK_CORRECTIONS_PRESENT:
        gnss.set_bit(GNSSstatus_networkCorrectionsPresent);
        break;
    }
  }
  return true;
}

bool brake_applied_status_asn2pb(
    const BrakeAppliedStatus_t* breaks_wheels_asn,
    google::protobuf::RepeatedField<int>* breaks_wheels_pb) {
  if (!breaks_wheels_asn || !breaks_wheels_pb) {
    LOG(INFO) << "args false";
    return false;
  }

  ASN1_HOLDER(BrakeAppliedStatus) const wheel(breaks_wheels_asn);

  if (wheel.test_bit(BrakeAppliedStatus_unavailable)) {
    breaks_wheels_pb->Add(::v2xpb::asn::BRAKE_APPLIED_UNAVAILABLE);
  }
  if (wheel.test_bit(BrakeAppliedStatus_leftFront)) {
    breaks_wheels_pb->Add(::v2xpb::asn::BRAKE_APPLIED_LEFTFRONT);
  }
  if (wheel.test_bit(BrakeAppliedStatus_leftRear)) {
    breaks_wheels_pb->Add(::v2xpb::asn::BRAKE_APPLIED_LEFTREAR);
  }
  if (wheel.test_bit(BrakeAppliedStatus_rightFront)) {
    breaks_wheels_pb->Add(::v2xpb::asn::BRAKE_APPLIED_RIGHTRFONT);
  }
  if (wheel.test_bit(BrakeAppliedStatus_rightRear)) {
    breaks_wheels_pb->Add(::v2xpb::asn::BRAKE_APPLIED_RIGHTREAR);
  }

  return true;
}

bool brake_applied_status_pb2asn(
    const google::protobuf::RepeatedField<int>& breaks_wheels_pb,
    BrakeAppliedStatus_t* breaks_wheels_asn) {
  if (!breaks_wheels_asn) {
    LOG(INFO) << "args false";
    return false;
  }
  ASN1_HOLDER(BrakeAppliedStatus) wheel(breaks_wheels_asn);
  wheel.init();

  for (const auto& it : breaks_wheels_pb) {
    switch (it) {
      case ::v2xpb::asn::BRAKE_APPLIED_UNAVAILABLE:
        wheel.set_bit(BrakeAppliedStatus_unavailable);
        break;
      case ::v2xpb::asn::BRAKE_APPLIED_LEFTFRONT:
        wheel.set_bit(BrakeAppliedStatus_leftFront);
        break;
      case ::v2xpb::asn::BRAKE_APPLIED_LEFTREAR:
        wheel.set_bit(BrakeAppliedStatus_leftRear);
        break;
      case ::v2xpb::asn::BRAKE_APPLIED_RIGHTRFONT:
        wheel.set_bit(BrakeAppliedStatus_rightFront);
        break;
      case ::v2xpb::asn::BRAKE_APPLIED_RIGHTREAR:
        wheel.set_bit(BrakeAppliedStatus_rightRear);
        break;
    }
  }
  return true;
}

bool utctime_asn2pb(const DDateTime_t* asn_utc,
                    ::v2xpb::asn::FullPositionVector_DDateTime* pb_utc) {
  if (!asn_utc || !pb_utc) {
    LOG(WARNING) << "args false";
    return false;
  }

  if (asn_utc->year) {
    pb_utc->set_year(static_cast<int>(*asn_utc->year));
  }
  if (asn_utc->month) {
    pb_utc->set_month(static_cast<int>(*asn_utc->month));
  }
  if (asn_utc->day) {
    pb_utc->set_day(static_cast<int>(*asn_utc->day));
  }
  if (asn_utc->hour) {
    pb_utc->set_hour(static_cast<int>(*asn_utc->hour));
  }
  if (asn_utc->minute) {
    pb_utc->set_minute(static_cast<int>(*asn_utc->minute));
  }
  if (asn_utc->second) {
    pb_utc->set_second(static_cast<int>(*asn_utc->second));
  }
  if (asn_utc->offset) {
    pb_utc->set_offset(static_cast<int>(*asn_utc->offset));
  }

  return true;
}

bool utctime_pb2asn(const ::v2xpb::asn::FullPositionVector_DDateTime& pb_utc,
                    DDateTime_t* asn_utc) {
  if (!asn_utc) {
    LOG(WARNING) << "args false";
    return false;
  }

  if (pb_utc.has_year()) {
    asn_utc->year = ASN1(DYear)::create_tp();
    *asn_utc->year = pb_utc.year();
  }
  if (pb_utc.has_month()) {
    asn_utc->month = ASN1(DMonth)::create_tp();
    *asn_utc->month = pb_utc.month();
  }
  if (pb_utc.has_day()) {
    asn_utc->day = ASN1(DDay)::create_tp();
    *asn_utc->day = pb_utc.day();
  }
  if (pb_utc.has_hour()) {
    asn_utc->hour = ASN1(DHour)::create_tp();
    *asn_utc->hour = pb_utc.hour();
  }
  if (pb_utc.has_minute()) {
    asn_utc->minute = ASN1(DMinute)::create_tp();
    *asn_utc->minute = pb_utc.minute();
  }
  if (pb_utc.has_second()) {
    asn_utc->second = ASN1(DSecond)::create_tp();
    *asn_utc->second = pb_utc.second();
  }
  if (pb_utc.has_offset()) {
    asn_utc->offset = ASN1(DTimeOffset)::create_tp();
    *asn_utc->offset = pb_utc.offset();
  }

  return true;
}

bool bsm_safety_asn2pb(const Position3D_t* asn_refpos,
                       const VehicleSafetyExtensions* asn_safety,
                       ::v2xpb::asn::VehicleSafetyExtensions* pb_safety) {
  if (!asn_safety || !pb_safety) {
    LOG(INFO) << "args false";
    return false;
  }

  if (asn_safety->events) {
    safety_events_asn2pb(asn_safety->events, pb_safety->mutable_events());
  }
  if (asn_safety->lights) {
    safety_lights_asn2pb(asn_safety->lights, pb_safety->mutable_lights());
  }

  if (asn_safety->pathPrediction) {
    pb_safety->mutable_path_prediction()->set_radius_ofcurve(
        static_cast<int>(asn_safety->pathPrediction->radiusOfCurve));
    pb_safety->mutable_path_prediction()->set_confidence(
        static_cast<int>(asn_safety->pathPrediction->confidence));
  }

  if (asn_safety->pathHistory) {
    auto asn_path_history = asn_safety->pathHistory;
    auto pb_path_history = pb_safety->mutable_path_history();
    if (asn_path_history->initialPosition) {
      auto pb_init_pos = pb_path_history->mutable_initial_position();
      auto asn_init_pos = asn_path_history->initialPosition;

      if (asn_init_pos->utcTime) {
        utctime_asn2pb(asn_init_pos->utcTime, pb_init_pos->mutable_utc_time());
      }

      position_asn2pb(&asn_init_pos->pos, pb_init_pos->mutable_pos());

      if (asn_init_pos->heading) {
        pb_init_pos->set_heading(static_cast<int>(*asn_init_pos->heading));
      }
      if (asn_init_pos->transmission) {
        pb_init_pos->set_transmission(
            static_cast<int>(*asn_init_pos->transmission));
      }
      if (asn_init_pos->speed) {
        pb_init_pos->set_speed(static_cast<int>(*asn_init_pos->speed));
      }

#if defined(CONFIG_ADAPTER_MODULE_asn_2_ENABLE)
      if (asn_init_pos->posConficence) {
        pb_init_pos->mutable_pos_accuracy()->set_pos(
            static_cast<int>(asn_init_pos->posConficence->pos));
        if (asn_init_pos->posConficence->elevation) {
          pb_init_pos->mutable_pos_accuracy()->set_elevation(
              static_cast<int>(*asn_init_pos->posConficence->elevation));
        }
      }
#endif

#if defined(CONFIG_ADAPTER_MODULE_asn_3_ENABLE)
      if (asn_init_pos->posAccuracy) {
        pb_init_pos->mutable_pos_accuracy()->set_pos(
            static_cast<int>(asn_init_pos->posAccuracy->pos));
        if (asn_init_pos->posAccuracy->elevation) {
          pb_init_pos->mutable_pos_accuracy()->set_elevation(
              static_cast<int>(*asn_init_pos->posAccuracy->elevation));
        }
      }
#endif

      if (asn_init_pos->timeConfidence) {
        pb_init_pos->set_time_confidence(
            static_cast<int>(*asn_init_pos->timeConfidence));
      }

      if (asn_init_pos->motionCfd) {
        motion_asn2pb(asn_init_pos->motionCfd,
                      pb_init_pos->mutable_motion_cfd());
      }
    }

    if (asn_path_history->currGNSSstatus) {
      gnss_status_asn2pb(asn_path_history->currGNSSstatus,
                         pb_path_history->mutable_curr_gnss_status());
    }

    auto asn_crumdata = asn_path_history->crumbData;
    for (int i = 0; i < asn_crumdata.list.count; i++) {
      auto asn_path_point = asn_crumdata.list.array[i];
      auto pb_path_point = pb_path_history->add_crumb_data();

      position_asn2pb(asn_refpos, &asn_path_point->llvOffset,
                      pb_path_point->mutable_llv_offset());

      pb_path_point->set_time_offset(
          static_cast<int>(asn_path_point->timeOffset));
      if (asn_path_point->speed) {
        pb_path_point->set_speed(static_cast<int>(*asn_path_point->speed));
      }

      if (asn_path_point->posAccuracy) {
        pb_path_point->mutable_pos_accuracy()->set_pos(
            static_cast<int>(asn_path_point->posAccuracy->pos));
        if (asn_path_point->posAccuracy->elevation) {
          pb_path_point->mutable_pos_accuracy()->set_elevation(
              static_cast<int>(*asn_path_point->posAccuracy->elevation));
        }
      }

      if (asn_path_point->heading) {
        pb_path_point->set_heading(static_cast<int>(*asn_path_point->heading));
      }
    }
  }

  return true;
}

bool bsm_safety_pb2asn(const Position3D_t* asn_refpos,
                       const ::v2xpb::asn::VehicleSafetyExtensions& pb_safety,
                       VehicleSafetyExtensions* asn_safety) {
  if (!asn_safety) {
    LOG(INFO) << "args false";
    return false;
  }

  if (pb_safety.events().size() > 0) {
    asn_safety->events = ASN1(VehicleEventFlags)::create_tp();
    safety_events_pb2asn(pb_safety.events(), asn_safety->events);
  }

  if (pb_safety.lights().size() > 0) {
    asn_safety->lights = ASN1(ExteriorLights)::create_tp();
    safety_lights_pb2asn(pb_safety.lights(), asn_safety->events);
  }

  if (pb_safety.has_path_prediction()) {
    asn_safety->pathPrediction = ASN1(PathPrediction)::create_tp();
    if (pb_safety.path_prediction().has_radius_ofcurve()) {
      asn_safety->pathPrediction->radiusOfCurve =
          pb_safety.path_prediction().radius_ofcurve();
    }
    if (pb_safety.path_prediction().has_confidence()) {
      asn_safety->pathPrediction->confidence =
          pb_safety.path_prediction().confidence();
    }
  }

  if (pb_safety.has_path_history()) {
    auto asn_path_history = ASN1(PathHistory)::create_tp();
    asn_safety->pathHistory = asn_path_history;
    const auto& pb_path_history = pb_safety.path_history();
    if (pb_path_history.has_initial_position()) {
      auto asn_init_pos = ASN1(FullPositionVector)::create_tp();
      asn_path_history->initialPosition = asn_init_pos;

      const auto& pb_init_pos = pb_path_history.initial_position();

      if (pb_init_pos.has_utc_time()) {
        asn_init_pos->utcTime = ASN1(DDateTime)::create_tp();
        utctime_pb2asn(pb_init_pos.utc_time(), asn_init_pos->utcTime);
      }

      position_pb2asn(pb_init_pos.pos(), &asn_init_pos->pos);

      if (pb_init_pos.has_heading()) {
        asn_init_pos->heading = ASN1(Heading)::create_tp();
        *asn_init_pos->heading = pb_init_pos.heading();
      }

      if (pb_init_pos.has_transmission()) {
        asn_init_pos->transmission = ASN1(TransmissionState)::create_tp();
        *asn_init_pos->transmission = pb_init_pos.transmission();
      }

      if (pb_init_pos.has_speed()) {
        asn_init_pos->speed = ASN1(Speed)::create_tp();
        *asn_init_pos->speed = pb_init_pos.speed();
      }

#if defined(CONFIG_ADAPTER_MODULE_asn_2_ENABLE)
      if (pb_init_pos.has_pos_accuracy()) {
        asn_init_pos->posConficence = ASN1(PositionConfidenceSet)::create_tp();
        asn_init_pos->posConficence->pos = pb_init_pos.pos_accuracy().pos();
        if (pb_init_pos.pos_accuracy().has_elevation()) {
          asn_init_pos->posConficence->elevation =
              ASN1(ElevationConfidence)::create_tp();
          *asn_init_pos->posConficence->elevation =
              pb_init_pos.pos_accuracy().elevation();
        }
      }
#endif

#if defined(CONFIG_ADAPTER_MODULE_asn_3_ENABLE)
      if (pb_init_pos.has_pos_accuracy()) {
        asn_init_pos->posAccuracy = ASN1(PositionConfidenceSet)::create_tp();
        asn_init_pos->posAccuracy->pos = pb_init_pos.pos_accuracy().pos();
        if (pb_init_pos.pos_accuracy().has_elevation()) {
          asn_init_pos->posAccuracy->elevation =
              ASN1(ElevationConfidence)::create_tp();
          *asn_init_pos->posAccuracy->elevation =
              pb_init_pos.pos_accuracy().elevation();
        }
      }
#endif

      if (pb_init_pos.has_time_confidence()) {
        asn_init_pos->timeConfidence = ASN1(TimeConfidence)::create_tp();
        *asn_init_pos->timeConfidence = pb_init_pos.time_confidence();
      }

      if (pb_init_pos.has_motion_cfd()) {
        asn_init_pos->motionCfd = ASN1(MotionConfidenceSet)::create_tp();
        motion_pb2asn(pb_init_pos.motion_cfd(), asn_init_pos->motionCfd);
      }
    }

    if (pb_path_history.curr_gnss_status().size() > 0) {
      asn_path_history->currGNSSstatus = ASN1(GNSSstatus)::create_tp();
      gnss_status_pb2asn(pb_path_history.curr_gnss_status(),
                         asn_path_history->currGNSSstatus);
    }

    for (const auto& pb_point : pb_path_history.crumb_data()) {
      auto asn_point = ASN1(PathHistoryPoint)::create_tp();
      ASN_SEQUENCE_ADD(&asn_path_history->crumbData.list, asn_point);

      position_pb2asn(asn_refpos, pb_point.llv_offset(), &asn_point->llvOffset);

      asn_point->timeOffset = pb_point.time_offset();
      if (pb_point.has_speed()) {
        asn_point->speed = ASN1(Speed)::create_tp();
        *asn_point->speed = pb_point.speed();
      }

      if (pb_point.has_pos_accuracy()) {
        asn_point->posAccuracy = ASN1(PositionConfidenceSet)::create_tp();
        asn_point->posAccuracy->pos = pb_point.pos_accuracy().pos();
        if (pb_point.pos_accuracy().has_elevation()) {
          asn_point->posAccuracy->elevation =
              ASN1(ElevationConfidence)::create_tp();
          *asn_point->posAccuracy->elevation =
              pb_point.pos_accuracy().elevation();
        }
      }

      if (pb_point.has_heading()) {
        asn_point->heading = ASN1(CoarseHeading)::create_tp();
        *asn_point->heading = pb_point.heading();
      }
    }
  }

  return true;
}

bool bsm_emergency_asn2pb(const VehicleEmergencyExtensions_t* asn_emer,
                          ::v2xpb::asn::VehicleEmergencyExtensions* pb_emer) {
  if (!asn_emer || !pb_emer) {
    LOG(WARNING) << "args false";
    return false;
  }

  if (asn_emer->responseType) {
    pb_emer->set_response_type(static_cast<int>(*asn_emer->responseType));
  }
  if (asn_emer->sirenUse) {
    pb_emer->set_siren_use(static_cast<int>(*asn_emer->sirenUse));
  }
  if (asn_emer->lightsUse) {
    pb_emer->set_lights_use(static_cast<int>(*asn_emer->lightsUse));
  }

  return true;
}

bool bsm_emergency_pb2asn(
    const ::v2xpb::asn::VehicleEmergencyExtensions& pb_emer,
    VehicleEmergencyExtensions_t* asn_emer) {
  if (!asn_emer) {
    LOG(WARNING) << "args false";
    return false;
  }

  if (pb_emer.has_response_type()) {
    asn_emer->responseType = ASN1(ResponseType)::create_tp();
    *asn_emer->responseType = pb_emer.response_type();
  }
  if (pb_emer.has_siren_use()) {
    asn_emer->sirenUse = ASN1(SirenInUse)::create_tp();
    *asn_emer->sirenUse = pb_emer.siren_use();
  }
  if (pb_emer.has_lights_use()) {
    asn_emer->lightsUse = ASN1(LightbarInUse)::create_tp();
    *asn_emer->lightsUse = pb_emer.lights_use();
  }

  return true;
}

bool bsm_brakes_asn2pb(const BrakeSystemStatus_t& asn_brakes,
                       ::v2xpb::asn::BrakeSystemStatus* pb_brakes) {
  if (!pb_brakes) {
    LOG(WARNING) << "args false";
    return false;
  }

  if (asn_brakes.brakePadel) {
    pb_brakes->set_brake_padel(static_cast<int>(*asn_brakes.brakePadel));
  }

  if (asn_brakes.wheelBrakes) {
    brake_applied_status_asn2pb(asn_brakes.wheelBrakes,
                                pb_brakes->mutable_wheel_brakes());
  }

  if (asn_brakes.traction) {
    pb_brakes->set_traction(static_cast<int>(*asn_brakes.traction));
  }

  if (asn_brakes.abs) {
    pb_brakes->set_abs(static_cast<int>(*asn_brakes.abs));
  }

  if (asn_brakes.scs) {
    pb_brakes->set_scs(static_cast<int>(*asn_brakes.scs));
  }

  if (asn_brakes.brakeBoost) {
    pb_brakes->set_brake_boost(static_cast<int>(*asn_brakes.brakeBoost));
  }

  if (asn_brakes.auxBrakes) {
    pb_brakes->set_aux_brakes(static_cast<int>(*asn_brakes.auxBrakes));
  }

  return true;
}

bool bsm_brakes_pb2asn(const ::v2xpb::asn::BrakeSystemStatus& pb_brakes,
                       BrakeSystemStatus_t* asn_brakes) {
  if (pb_brakes.has_brake_padel()) {
    auto data = pb_brakes.brake_padel();
    if (data >= 0 && data <= 2) {
      asn_brakes->brakePadel = ASN1(BrakePedalStatus)::create_tp();
      *asn_brakes->brakePadel = data;
    }
  }

  if (pb_brakes.wheel_brakes().size() > 0) {
    asn_brakes->wheelBrakes = ASN1(BrakeAppliedStatus)::create_tp();
    brake_applied_status_pb2asn(pb_brakes.wheel_brakes(),
                                asn_brakes->wheelBrakes);
  }

  if (pb_brakes.has_traction()) {
    auto data = pb_brakes.traction();
    if (data >= 0 && data <= 3) {
      asn_brakes->traction = ASN1(TractionControlStatus)::create_tp();
      *asn_brakes->traction = data;
    }
  }

  if (pb_brakes.has_abs()) {
    auto data = pb_brakes.abs();
    if (data >= 0 && data <= 3) {
      asn_brakes->abs = ASN1(AntiLockBrakeStatus)::create_tp();
      *asn_brakes->abs = data;
    }
  }

  if (pb_brakes.has_scs()) {
    auto data = pb_brakes.scs();
    if (data >= 0 && data <= 3) {
      asn_brakes->scs = ASN1(StabilityControlStatus)::create_tp();
      *asn_brakes->scs = data;
    }
  }

  if (pb_brakes.has_brake_boost()) {
    auto data = pb_brakes.brake_boost();
    if (data >= 0 && data <= 2) {
      asn_brakes->brakeBoost = ASN1(BrakeBoostApplied)::create_tp();
      *asn_brakes->brakeBoost = data;
    }
  }

  if (pb_brakes.has_aux_brakes()) {
    auto data = pb_brakes.aux_brakes();
    if (data >= 0 && data <= 3) {
      asn_brakes->auxBrakes = ASN1(AuxiliaryBrakeStatus)::create_tp();
      *asn_brakes->auxBrakes = data;
    }
  }

  return true;
}

bool bsm_asn2pb(const BasicSafetyMessage_t* bsm_asn,
                ::v2xpb::asn::Bsm* bsm_pb) {
  if (!bsm_asn || !bsm_pb) {
    LOG(INFO) << "args false";
    return false;
  }

  bsm_pb->set_msg_cnt(static_cast<int>(bsm_asn->msgCnt));
  bsm_pb->set_id((const char*)bsm_asn->id.buf, bsm_asn->id.size);
  bsm_pb->set_sec_mark(static_cast<int>(bsm_asn->secMark));

  position_asn2pb(&bsm_asn->pos, bsm_pb->mutable_pos());

  if (bsm_asn->timeConfidence) {
    bsm_pb->set_time_confidence(static_cast<int>(*bsm_asn->timeConfidence));
  }
  if (bsm_asn->posAccuracy) {
    bsm_pb->mutable_pos_accuracy()->set_semi_major(
        static_cast<int>(bsm_asn->posAccuracy->semiMajor));
    bsm_pb->mutable_pos_accuracy()->set_semi_minor(
        static_cast<int>(bsm_asn->posAccuracy->semiMinor));
    bsm_pb->mutable_pos_accuracy()->set_orientation(
        static_cast<int>(bsm_asn->posAccuracy->orientation));
  }
  if (bsm_asn->posConfidence) {
    bsm_pb->mutable_pos_confidence()->set_pos(
        static_cast<int>(bsm_asn->posConfidence->pos));
    if (bsm_asn->posConfidence->elevation) {
      bsm_pb->mutable_pos_confidence()->set_elevation(
          static_cast<int>(*bsm_asn->posConfidence->elevation));
    }
  }

  bsm_pb->set_transmission(static_cast<int>(bsm_asn->transmission));
  bsm_pb->set_speed(static_cast<int>(bsm_asn->speed));
  bsm_pb->set_heading(static_cast<int>(bsm_asn->heading));
  if (bsm_asn->angle) {
    bsm_pb->set_angle(static_cast<int>(*bsm_asn->angle));
  }

  motion_asn2pb(bsm_asn->motionCfd, bsm_pb->mutable_motion_cfd());

  auto pb_accel = bsm_pb->mutable_accel_set();
  pb_accel->set_lon(static_cast<int>(bsm_asn->accelSet.Long));
  pb_accel->set_lat(static_cast<int>(bsm_asn->accelSet.lat));
  pb_accel->set_vert(static_cast<int>(bsm_asn->accelSet.vert));
  pb_accel->set_yaw(static_cast<int>(bsm_asn->accelSet.yaw));

  bsm_brakes_asn2pb(bsm_asn->brakes, bsm_pb->mutable_brakes());

  bsm_pb->mutable_size()->set_width(static_cast<int>(bsm_asn->size.width));
  bsm_pb->mutable_size()->set_length(static_cast<int>(bsm_asn->size.length));
  if (bsm_asn->size.height) {
    bsm_pb->mutable_size()->set_height(static_cast<int>(*bsm_asn->size.height));
  }

  bsm_pb->mutable_vehicle_class()->set_classification(
      static_cast<int>(bsm_asn->vehicleClass.classification));
  if (bsm_asn->vehicleClass.fuelType) {
    bsm_pb->mutable_vehicle_class()->set_fuel_type(
        static_cast<int>(*bsm_asn->vehicleClass.fuelType));
  }

  if (bsm_asn->safetyExt) {
    bsm_safety_asn2pb(&bsm_asn->pos, bsm_asn->safetyExt,
                      bsm_pb->mutable_safety_ext());
  }

  if (bsm_asn->emergencyExt) {
    bsm_emergency_asn2pb(bsm_asn->emergencyExt,
                         bsm_pb->mutable_emergency_ext());
  }

  return true;
}

bool bsm_pb2asn(const ::v2xpb::asn::Bsm& bsm_pb,
                BasicSafetyMessage_t* bsm_asn) {
  if (!bsm_asn) {
    LOG(INFO) << "args false";
    return false;
  }

  if (bsm_pb.has_msg_cnt()) {
    bsm_asn->msgCnt = bsm_pb.msg_cnt();
  }
  if (bsm_pb.has_id()) {
    if (bsm_pb.id().length() != 8) {
      LOG(WARNING) << "bsm id size not 8:" << bsm_pb.id().length();
      return false;
    }

    OCTET_STRING_fromBuf(&bsm_asn->id, bsm_pb.id().c_str(),
                         bsm_pb.id().length());
  }
  if (bsm_pb.has_sec_mark()) {
    bsm_asn->secMark = bsm_pb.sec_mark();
  }
  if (bsm_pb.has_pos()) {
    position_pb2asn(bsm_pb.pos(), &bsm_asn->pos);
  }

  if (bsm_pb.has_time_confidence()) {
    bsm_asn->timeConfidence = ASN1(TimeConfidence)::create_tp();
    *bsm_asn->timeConfidence = bsm_pb.time_confidence();
  }

  if (bsm_pb.has_pos_accuracy()) {
    bsm_asn->posAccuracy = ASN1(PositionalAccuracy)::create_tp();
    if (bsm_pb.pos_accuracy().has_semi_major()) {
      bsm_asn->posAccuracy->semiMajor = bsm_pb.pos_accuracy().semi_major();
    }
    if (bsm_pb.pos_accuracy().has_semi_minor()) {
      bsm_asn->posAccuracy->semiMinor = bsm_pb.pos_accuracy().semi_minor();
    }
    if (bsm_pb.pos_accuracy().has_orientation()) {
      bsm_asn->posAccuracy->orientation = bsm_pb.pos_accuracy().orientation();
    }
  }

  if (bsm_pb.has_pos_confidence()) {
    bsm_asn->posConfidence = ASN1(PositionConfidenceSet)::create_tp();
    if (bsm_pb.pos_confidence().has_pos()) {
      bsm_asn->posConfidence->pos = bsm_pb.pos_confidence().pos();
    }

    if (bsm_pb.pos_confidence().has_elevation()) {
      bsm_asn->posConfidence->elevation =
          ASN1(ElevationConfidence)::create_tp();
      *bsm_asn->posConfidence->elevation = bsm_pb.pos_confidence().elevation();
    }
  }

  if (bsm_pb.has_transmission()) {
    bsm_asn->transmission = bsm_pb.transmission();
  }

  if (bsm_pb.has_speed()) {
    bsm_asn->speed = bsm_pb.speed();
  }

  if (bsm_pb.has_heading()) {
    bsm_asn->heading = bsm_pb.heading();
  }

  if (bsm_pb.has_angle()) {
    bsm_asn->angle = ASN1(SteeringWheelAngle)::create_tp();
    *bsm_asn->angle = bsm_pb.angle();
  }

  if (bsm_pb.has_motion_cfd()) {
    bsm_asn->motionCfd = ASN1(MotionConfidenceSet)::create_tp();
    motion_pb2asn(bsm_pb.motion_cfd(), bsm_asn->motionCfd);
  }

  if (bsm_pb.has_accel_set()) {
    if (bsm_pb.accel_set().has_lon()) {
      bsm_asn->accelSet.Long = bsm_pb.accel_set().lon();
    }
    if (bsm_pb.accel_set().has_lat()) {
      bsm_asn->accelSet.lat = bsm_pb.accel_set().lat();
    }
    if (bsm_pb.accel_set().has_vert()) {
      bsm_asn->accelSet.vert = bsm_pb.accel_set().vert();
    }
    if (bsm_pb.accel_set().yaw()) {
      bsm_asn->accelSet.yaw = bsm_pb.accel_set().yaw();
    }
  }

  if (bsm_pb.has_brakes()) {
    bsm_brakes_pb2asn(bsm_pb.brakes(), &bsm_asn->brakes);
  }

  if (bsm_pb.has_size()) {
    if (bsm_pb.size().has_width()) {
      bsm_asn->size.width = bsm_pb.size().width();
    }
    if (bsm_pb.size().has_length()) {
      bsm_asn->size.length = bsm_pb.size().length();
    }
    if (bsm_pb.size().has_height()) {
      bsm_asn->size.height = ASN1(VehicleHeight)::create_tp();
      *bsm_asn->size.height = bsm_pb.size().height();
    }
  }

  if (bsm_pb.has_vehicle_class()) {
    if (bsm_pb.vehicle_class().has_classification()) {
      bsm_asn->vehicleClass.classification =
          bsm_pb.vehicle_class().classification();
    }
    if (bsm_pb.vehicle_class().has_fuel_type()) {
      bsm_asn->vehicleClass.fuelType = ASN1(FuelType)::create_tp();
      *bsm_asn->vehicleClass.fuelType = bsm_pb.vehicle_class().fuel_type();
    }
  }

  if (bsm_pb.has_safety_ext()) {
    bsm_asn->safetyExt = ASN1(VehicleSafetyExtensions)::create_tp();
    bsm_safety_pb2asn(&bsm_asn->pos, bsm_pb.safety_ext(), bsm_asn->safetyExt);
  }

  if (bsm_pb.has_emergency_ext()) {
    bsm_asn->emergencyExt = ASN1(VehicleEmergencyExtensions)::create_tp();
    bsm_emergency_pb2asn(bsm_pb.emergency_ext(), bsm_asn->emergencyExt);
  }

  return true;
}
